package frc.circuitrunners;

/*
 * StartApplication.java
 */
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.Watchdog;
import frc.circuitrunners.IO.IO;
import frc.circuitrunners.RobotSubsystems.DriveTrain.Drive;
import frc.circuitrunners.RobotSubsystems.Conveyor.Conveyor;
import frc.circuitrunners.RobotSubsystems.RobotSubsystem;
import frc.circuitrunners.RobotSubsystems.Shooter.Shooter;

/**
 * The main scheduler for the robot, work 
 */
public class Scheduler extends RobotBase {
    
    /**
     * Holds whether the autonomous has executed once
     */
    private boolean cpuFlag = true;
    
    /**
     * How many of the subsystem have completed the task for the current stage during hybrid
     */
    private byte flagByte = 0;
    
    /**
     * Number of stages, or specific actions made during hybrid
     */
    private final byte MAX_NUM_STAGES = 3; //however number of stages we have for auto
    
    /**
     * Subsystems that the robot utilizes in its execution
     */
    private RobotSubsystem[] fibers = {
        new Drive()//,
        //new Fetcher(),
        //new Shooter()
    };
    
    /**
     * Watchdog, for safety purposes, makes sure the robot is executing as prescribed
     */
    public static Watchdog watchdog;
    
    /**
     * Current subsystem the robot is giving commands to
     */
    private byte currentFiber = 0;
    
    /**
     * Main Loop
     */
    public void startCompetition() {
        IO.init();
        watchdog = getWatchdog();
        for(int i = 0; i < fibers.length; i++){
            fibers[i].init();
        }
        while(true){
            feed();
            if(isEnabled()){ //Only when Enabled
                //if(isAutonomous()){ // Hybrid Period
                //    autonomous();
                //} else { //Operator Control Period
                    //if(R.AUTO_AIM){
                       //autoAim();
                    //}else{
                       teleop();
                    //}
                //}
            } else { //when diabled
                disabled();
            }
            if(++currentFiber > fibers.length){
                currentFiber = 0;
            }
        }
    }
    
    /**
     * Returns to the main loop, moves onto the next fiber
     */
    public static void yield(){
        throw new schedulerReturnException();
    }
    
    /**
     * feeds the watchdog
     */
    public static void feed(){
        watchdog.feed();
    }
    
    /**
     * Used to return to the scheduler
     */
    static class schedulerReturnException extends RuntimeException{}
    
    /**
     * Robot code for hybrid mode
     */
    private void autonomous(){       
        try{
            if(cpuFlag){
                try{
                    fibers[currentFiber].runAutonomous();//run the fiber's autonomous program based on the stage
                }catch(schedulerReturnException e){
                    //ignore, perfectly normal
                } 
                for(int i = 0; i < fibers.length; i++)
                    if(fibers[i].getFlag())
                        flagByte++;
                if(flagByte >= fibers.length){ //Once all fibers are done with the stage
                    R.STAGE++; //Increments process to the next stage
                    for(int j = 0; j < fibers.length; j++){
                        fibers[j].setFlag(false);//Resets all the fibers' flags
                    }
                }
                flagByte = 0;
                cpuFlag = (R.STAGE < MAX_NUM_STAGES); //Stops autonomous if it has passed all the stages
            }
        }catch(Exception e){
            IO.log(e.getMessage(), "Error in Autonomous!");
        }catch(Error e){
            IO.log(e.getMessage(), "Error in Autonomous!");
        }
    }
    
    /**
     * Robot code for teleop
     */
    private void teleop(){
        try{
            fibers[currentFiber].runTeleop();
        }catch(schedulerReturnException e){
            //Ignore, perfectly normal
        }catch(Exception e){
            IO.log(e.getMessage(), "Error in Teleoperated!");
        }catch(Error e){
            IO.log(e.getMessage(), "Error in Teleoperated!");
        }
    }
    
    /**
     * Robot code for when disabled
     */
    private void disabled(){
        try{
            fibers[currentFiber].disable();
        }catch(schedulerReturnException e){
            //Ignore, perfectly normal
        }catch(Exception e){
            IO.log(e.getMessage(), "Error in Disable!");
        }catch(Error e){
            IO.log(e.getMessage(), "Error in Disable!");
        }
    }
    
    /**
     * Game
     */
    private void autoAim(){
        try{
            fibers[currentFiber].autoAim();
        }catch(schedulerReturnException e){
            //Ignore, perfectly normal
        }catch(Exception e){
            IO.log(e.getMessage(), "Error in Autoaim!");
        }catch(Error e){
            IO.log(e.getMessage(), "Error in Autoaim!");
        }
    }
}